#include "Box.h"
#include "Bounds.h"
#include "Sphere.h"
#include "Plane.h"
#include "Math.h"

namespace Core
{
	inline rtBox::rtBox(void) 
	{
	}

	inline rtBox::rtBox(const rtVector3& center, const rtVector3& extents, const rtMatrix3& axis) 
	{
		this->m_Center = center;
		this->m_Extents = extents;
		this->m_Axis = axis;
	}

	inline rtBox::rtBox(const rtVector3& point) 
	{
		this->m_Center = point;
		this->m_Extents.zero();
		this->m_Axis.identity();
	}

	inline rtBox::rtBox(const rtBounds& bounds) 
	{
		this->m_Center = (bounds[0] + bounds[1]) * 0.5f;
		this->m_Extents = bounds[1] - this->m_Center;
		this->m_Axis.identity();
	}

	inline rtBox::rtBox(const rtBounds& bounds, const rtVector3& origin, const rtMatrix3& m_Axis) 
	{
		this->m_Center = (bounds[0] + bounds[1]) * 0.5f;
		this->m_Extents = bounds[1] - this->m_Center;
		this->m_Center = origin + this->m_Center * m_Axis;
		this->m_Axis = m_Axis;
	}

	inline rtBox rtBox::operator + (const rtVector3& t) const 
	{
		return rtBox(m_Center + t, m_Extents, m_Axis);
	}

	inline rtBox& rtBox::operator += (const rtVector3& t) 
	{
		m_Center += t;
		return *this;
	}

	inline rtBox rtBox::operator * (const rtMatrix3& r) const 
	{
		return rtBox(m_Center * r, m_Extents, m_Axis * r);
	}

	inline rtBox& rtBox::operator *= (const rtMatrix3& r) 
	{
		m_Center *= r;
		m_Axis *= r;
		return *this;
	}

	inline rtBox rtBox::operator + (const rtBox& a) const 
	{
		rtBox newBox;
		newBox = *this;
		newBox.addBox(a);
		return newBox;
	}

	inline rtBox& rtBox::operator += (const rtBox& a) 
	{
		rtBox::addBox(a);
		return *this;
	}

	inline rtBox rtBox::operator - (const rtBox& a) const 
	{
		return rtBox(m_Center, m_Extents - a.m_Extents, m_Axis);
	}

	inline rtBox& rtBox::operator -= (const rtBox& a) 
	{
		m_Extents -= a.m_Extents;
		return *this;
	}

	inline bool rtBox::compare(const rtBox& a) const 
	{
		return (m_Center.compare(a.m_Center) && m_Extents.compare(a.m_Extents) && m_Axis.compare(a.m_Axis));
	}

	inline bool rtBox::compare(const rtBox& a, const float epsilon) const 
	{
		return (m_Center.compare(a.m_Center, epsilon) && m_Extents.compare(a.m_Extents, epsilon) && m_Axis.compare(a.m_Axis, epsilon));
	}

	inline bool rtBox::operator==(const rtBox& a) const 
	{
		return compare(a);
	}

	inline bool rtBox::operator!=(const rtBox& a) const 
	{
		return !compare(a);
	}

	inline void rtBox::clear(void) 
	{
		m_Center.zero();
		m_Extents[0] = m_Extents[1] = m_Extents[2] = -rtMath::INFINITY;
		m_Axis.identity();
	}

	inline void rtBox::zero(void) 
	{
		m_Center.zero();
		m_Extents.zero();
		m_Axis.identity();
	}

	inline const rtVector3& rtBox::getCenter(void) const 
	{
		return m_Center;
	}

	inline const rtVector3& rtBox::getExtents(void) const 
	{
		return m_Extents;
	}

	inline const rtMatrix3& rtBox::getAxis(void) const 
	{
		return m_Axis;
	}

	inline float rtBox::getVolume(void) const 
	{
		return (m_Extents * 2.0f).lengthSqr();
	}

	inline bool rtBox::isCleared(void) const 
	{
		return m_Extents[0] < 0.0f;
	}

	inline rtBox rtBox::expand(const float d) const 
	{
		return rtBox(m_Center, m_Extents + rtVector3(d, d, d), m_Axis);
	}

	inline rtBox& rtBox::expandSelf(const float d) 
	{
		m_Extents[0] += d;
		m_Extents[1] += d;
		m_Extents[2] += d;
		return *this;
	}

	inline rtBox rtBox::translate(const rtVector3& translation) const 
	{
		return rtBox(m_Center + translation, m_Extents, m_Axis);
	}

	inline rtBox& rtBox::translateSelf(const rtVector3& translation) 
	{
		m_Center += translation;
		return *this;
	}

	inline rtBox rtBox::rotate(const rtMatrix3& rotation) const 
	{
		return rtBox(m_Center * rotation, m_Extents, m_Axis * rotation);
	}

	inline rtBox& rtBox::rotateSelf(const rtMatrix3& rotation) 
	{
		m_Center *= rotation;
		m_Axis *= rotation;
		return *this;
	}

	inline bool rtBox::containsPoint(const rtVector3& p) const 
	{
		rtVector3 lp = p - m_Center;
		if (rtMath::Fabs(lp * m_Axis[0]) > m_Extents[0] ||
			rtMath::Fabs(lp * m_Axis[1]) > m_Extents[1] ||
			rtMath::Fabs(lp * m_Axis[2]) > m_Extents[2]) 
		{
				return false;
		}
		return true;
	}

	inline rtSphere rtBox::toSphere(void) const 
	{
		return rtSphere(m_Center, m_Extents.length());
	}

	inline void rtBox::axisProjection(const rtVector3& dir, float& min, float& max) const 
	{
		float d1 = dir * m_Center;
		float d2 = rtMath::Fabs(m_Extents[0] * (dir * m_Axis[0])) +
			       rtMath::Fabs(m_Extents[1] * (dir * m_Axis[1])) +
			       rtMath::Fabs(m_Extents[2] * (dir * m_Axis[2]));
		min = d1 - d2;
		max = d1 + d2;
	}

	inline void rtBox::axisProjection(const rtMatrix3& ax, rtBounds& bounds) const 
	{
		for (int i = 0; i < 3; i++) 
		{
			float d1 = ax[i] * m_Center;
			float d2 = rtMath::Fabs(m_Extents[0] * (ax[i] * m_Axis[0])) +
				       rtMath::Fabs(m_Extents[1] * (ax[i] * m_Axis[1])) +
				       rtMath::Fabs(m_Extents[2] * (ax[i] * m_Axis[2]));
			bounds[0][i] = d1 - d2;
			bounds[1][i] = d1 + d2;
		}
	}

	bool rtBox::addPoint(const rtVector3& v) 
	{
		rtMatrix3 axis2;
		rtBounds bounds1, bounds2;

		if (m_Extents[0] < 0.0f) 
		{
			m_Extents.zero();
			m_Center = v;
			m_Axis.identity();
			return true;
		}

		bounds1[0][0] = bounds1[1][0] = m_Center * m_Axis[0];
		bounds1[0][1] = bounds1[1][1] = m_Center * m_Axis[1];
		bounds1[0][2] = bounds1[1][2] = m_Center * m_Axis[2];
		bounds1[0] -= m_Extents;
		bounds1[1] += m_Extents;
		if (!bounds1.addPoint(rtVector3(v * m_Axis[0], v * m_Axis[1], v * m_Axis[2]))) 
		{
			return false;
		}

		axis2[0] = v - m_Center;
		axis2[0].normalize();
		axis2[1] = m_Axis[Min3Index(axis2[0] * m_Axis[0], axis2[0] * m_Axis[1], axis2[0] * m_Axis[2])];
		axis2[1] = axis2[1] - (axis2[1] * axis2[0]) * axis2[0];
		axis2[1].normalize();
		axis2[2].cross(axis2[0], axis2[1]);

		axisProjection(axis2, bounds2);
		bounds2.addPoint(rtVector3(v * axis2[0], v * axis2[1], v * axis2[2]));

		if (bounds1.getVolume() < bounds2.getVolume()) 
		{
			m_Center = (bounds1[0] + bounds1[1]) * 0.5f;
			m_Extents = bounds1[1] - m_Center;
			m_Center *= m_Axis;
		}
		else
		{
			m_Center = (bounds2[0] + bounds2[1]) * 0.5f;
			m_Extents = bounds2[1] - m_Center;
			m_Center *= axis2;
			m_Axis = axis2;
		}
		return true;
	}


	bool rtBox::addBox(const rtBox& a) 
	{
		int i, besti;
		float v, bestv;
		rtVector3 dir;
		rtMatrix3 ax[4];
		rtBounds bounds[4], b;

		if (a.m_Extents[0] < 0.0f)
		{
			return false;
		}

		if (m_Extents[0] < 0.0f) 
		{
			m_Center = a.m_Center;
			m_Extents = a.m_Extents;
			m_Axis = a.m_Axis;
			return true;
		}

		ax[0] = m_Axis;
		bounds[0][0][0] = bounds[0][1][0] = m_Center * ax[0][0];
		bounds[0][0][1] = bounds[0][1][1] = m_Center * ax[0][1];
		bounds[0][0][2] = bounds[0][1][2] = m_Center * ax[0][2];
		bounds[0][0] -= m_Extents;
		bounds[0][1] += m_Extents;
		a.axisProjection(ax[0], b);
		if (!bounds[0].addBounds(b)) 
		{
			return false;
		}

		ax[1] = a.m_Axis;
		bounds[1][0][0] = bounds[1][1][0] = a.m_Center * ax[1][0];
		bounds[1][0][1] = bounds[1][1][1] = a.m_Center * ax[1][1];
		bounds[1][0][2] = bounds[1][1][2] = a.m_Center * ax[1][2];
		bounds[1][0] -= a.m_Extents;
		bounds[1][1] += a.m_Extents;
		axisProjection(ax[1], b);
		if (!bounds[1].addBounds(b)) 
		{
			m_Center = a.m_Center;
			m_Extents = a.m_Extents;
			m_Axis = a.m_Axis;
			return true;
		}

		dir = a.m_Center - m_Center;
		dir.normalize();
		for (i = 2; i < 4; i++) 
		{
			ax[i][0] = dir;
			ax[i][1] = ax[i-2][ Min3Index(dir * ax[i-2][0], dir * ax[i-2][1], dir * ax[i-2][2]) ];
			ax[i][1] = ax[i][1] - (ax[i][1] * dir) * dir;
			ax[i][1].normalize();
			ax[i][2].cross(dir, ax[i][1]);

			axisProjection(ax[i], bounds[i]);
			a.axisProjection(ax[i], b);
			bounds[i].addBounds(b);
		}

		bestv = rtMath::INFINITY;
		besti = 0;
		for (i = 0; i < 4; i++) 
		{
			v = bounds[i].getVolume();
			if (v < bestv) 
			{
				bestv = v;
				besti = i;
			}
		}

		m_Center = (bounds[besti][0] + bounds[besti][1]) * 0.5f;
		m_Extents = bounds[besti][1] - m_Center;
		m_Center *= ax[besti];
		m_Axis = ax[besti];

		return false;
	}

	float rtBox::planeDistance(const rtPlane& plane) const 
	{
		float d1, d2;

		d1 = plane.distance(m_Center);
		d2 = rtMath::Fabs(m_Extents[0] * plane.normal()[0]) +
			rtMath::Fabs(m_Extents[1] * plane.normal()[1]) +
			rtMath::Fabs(m_Extents[2] * plane.normal()[2]);

		if (d1 - d2 > 0.0f) {
			return d1 - d2;
		}
		if (d1 + d2 < 0.0f) {
			return d1 + d2;
		}
		return 0.0f;
	}


	int rtBox::planeSide(const rtPlane& plane, const float epsilon) const 
	{
		float d1, d2;

		d1 = plane.distance(m_Center);
		d2 = rtMath::Fabs(m_Extents[0] * plane.normal()[0]) +
			 rtMath::Fabs(m_Extents[1] * plane.normal()[1]) +
			 rtMath::Fabs(m_Extents[2] * plane.normal()[2]);

		if (d1 - d2 > epsilon) 
		{
			return PLANESIDE_FRONT;
		}
		if (d1 + d2 < -epsilon) 
		{
			return PLANESIDE_BACK;
		}
		return PLANESIDE_CROSS;
	}


	bool rtBox::intersectsBox(const rtBox& a) const 
	{
		rtVector3 dir;
		float c[3][3];
		float ac[3][3];
		float axisdir[3];
		float d, e0, e1;

		dir = a.m_Center - m_Center;


		c[0][0] = m_Axis[0] * a.m_Axis[0];
		c[0][1] = m_Axis[0] * a.m_Axis[1];
		c[0][2] = m_Axis[0] * a.m_Axis[2];
		axisdir[0] = m_Axis[0] * dir;
		ac[0][0] = rtMath::Fabs(c[0][0]);
		ac[0][1] = rtMath::Fabs(c[0][1]);
		ac[0][2] = rtMath::Fabs(c[0][2]);

		d = rtMath::Fabs(axisdir[0]);
		e0 = m_Extents[0];
		e1 = a.m_Extents[0] * ac[0][0] + a.m_Extents[1] * ac[0][1] + a.m_Extents[2] * ac[0][2];
		if (d > e0 + e1) 
		{
			return false;
		}

		c[1][0] = m_Axis[1] * a.m_Axis[0];
		c[1][1] = m_Axis[1] * a.m_Axis[1];
		c[1][2] = m_Axis[1] * a.m_Axis[2];
		axisdir[1] = m_Axis[1] * dir;
		ac[1][0] = rtMath::Fabs(c[1][0]);
		ac[1][1] = rtMath::Fabs(c[1][1]);
		ac[1][2] = rtMath::Fabs(c[1][2]);

		d = rtMath::Fabs(axisdir[1]);
		e0 = m_Extents[1];
		e1 = a.m_Extents[0] * ac[1][0] + a.m_Extents[1] * ac[1][1] + a.m_Extents[2] * ac[1][2];
		if (d > e0 + e1) 
		{
			return false;
		}


		c[2][0] = m_Axis[2] * a.m_Axis[0];
		c[2][1] = m_Axis[2] * a.m_Axis[1];
		c[2][2] = m_Axis[2] * a.m_Axis[2];
		axisdir[2] = m_Axis[2] * dir;
		ac[2][0] = rtMath::Fabs(c[2][0]);
		ac[2][1] = rtMath::Fabs(c[2][1]);
		ac[2][2] = rtMath::Fabs(c[2][2]);

		d = rtMath::Fabs(axisdir[2]);
		e0 = m_Extents[2];
		e1 = a.m_Extents[0] * ac[2][0] + a.m_Extents[1] * ac[2][1] + a.m_Extents[2] * ac[2][2];
		if (d > e0 + e1) 
		{
			return false;
		}


		d = rtMath::Fabs(a.m_Axis[0] * dir);
		e0 = m_Extents[0] * ac[0][0] + m_Extents[1] * ac[1][0] + m_Extents[2] * ac[2][0];
		e1 = a.m_Extents[0];
		if (d > e0 + e1) 
		{
			return false;
		}


		d = rtMath::Fabs(a.m_Axis[1] * dir);
		e0 = m_Extents[0] * ac[0][1] + m_Extents[1] * ac[1][1] + m_Extents[2] * ac[2][1];
		e1 = a.m_Extents[1];
		if (d > e0 + e1) 
		{
			return false;
		}


		d = rtMath::Fabs(a.m_Axis[2] * dir);
		e0 = m_Extents[0] * ac[0][2] + m_Extents[1] * ac[1][2] + m_Extents[2] * ac[2][2];
		e1 = a.m_Extents[2];
		if (d > e0 + e1) 
		{
			return false;
		}


		d = rtMath::Fabs(axisdir[2] * c[1][0] - axisdir[1] * c[2][0]);
		e0 = m_Extents[1] * ac[2][0] + m_Extents[2] * ac[1][0];
		e1 = a.m_Extents[1] * ac[0][2] + a.m_Extents[2] * ac[0][1];
		if (d > e0 + e1) 
		{
			return false;
		}


		d = rtMath::Fabs(axisdir[2] * c[1][1] - axisdir[1] * c[2][1]);
		e0 = m_Extents[1] * ac[2][1] + m_Extents[2] * ac[1][1];
		e1 = a.m_Extents[0] * ac[0][2] + a.m_Extents[2] * ac[0][0];
		if (d > e0 + e1) 
		{
			return false;
		}


		d = rtMath::Fabs(axisdir[2] * c[1][2] - axisdir[1] * c[2][2]);
		e0 = m_Extents[1] * ac[2][2] + m_Extents[2] * ac[1][2];
		e1 = a.m_Extents[0] * ac[0][1] + a.m_Extents[1] * ac[0][0];
		if (d > e0 + e1) 
		{
			return false;
		}


		d = rtMath::Fabs(axisdir[0] * c[2][0] - axisdir[2] * c[0][0]);
		e0 = m_Extents[0] * ac[2][0] + m_Extents[2] * ac[0][0];
		e1 = a.m_Extents[1] * ac[1][2] + a.m_Extents[2] * ac[1][1];
		if (d > e0 + e1) 
		{
			return false;
		}


		d = rtMath::Fabs(axisdir[0] * c[2][1] - axisdir[2] * c[0][1]);
		e0 = m_Extents[0] * ac[2][1] + m_Extents[2] * ac[0][1];
		e1 = a.m_Extents[0] * ac[1][2] + a.m_Extents[2] * ac[1][0];
		if (d > e0 + e1) 
		{
			return false;
		}


		d = rtMath::Fabs(axisdir[0] * c[2][2] - axisdir[2] * c[0][2]);
		e0 = m_Extents[0] * ac[2][2] + m_Extents[2] * ac[0][2];
		e1 = a.m_Extents[0] * ac[1][1] + a.m_Extents[1] * ac[1][0];
		if (d > e0 + e1) 
		{
			return false;
		}


		d = rtMath::Fabs(axisdir[1] * c[0][0] - axisdir[0] * c[1][0]);
		e0 = m_Extents[0] * ac[1][0] + m_Extents[1] * ac[0][0];
		e1 = a.m_Extents[1] * ac[2][2] + a.m_Extents[2] * ac[2][1];
		if (d > e0 + e1) 
		{
			return false;
		}


		d = rtMath::Fabs(axisdir[1] * c[0][1] - axisdir[0] * c[1][1]);
		e0 = m_Extents[0] * ac[1][1] + m_Extents[1] * ac[0][1];
		e1 = a.m_Extents[0] * ac[2][2] + a.m_Extents[2] * ac[2][0];
		if (d > e0 + e1) 
		{
			return false;
		}


		d = rtMath::Fabs(axisdir[1] * c[0][2] - axisdir[0] * c[1][2]);
		e0 = m_Extents[0] * ac[1][2] + m_Extents[1] * ac[0][2];
		e1 = a.m_Extents[0] * ac[2][1] + a.m_Extents[1] * ac[2][0];
		if (d > e0 + e1) 
		{
			return false;
		}
		return true;
	}

	bool rtBox::lineIntersection(const rtVector3& start, const rtVector3& end) const 
	{
		float ld[3];
		rtVector3 lineDir = 0.5f * (end - start);
		rtVector3 lineCenter = start + lineDir;
		rtVector3 dir = lineCenter - m_Center;

		ld[0] = rtMath::Fabs(lineDir * m_Axis[0]);
		if (rtMath::Fabs(dir * m_Axis[0]) > m_Extents[0] + ld[0]) 
		{
			return false;
		}

		ld[1] = rtMath::Fabs(lineDir * m_Axis[1]);
		if (rtMath::Fabs(dir * m_Axis[1]) > m_Extents[1] + ld[1]) 
		{
			return false;
		}

		ld[2] = rtMath::Fabs(lineDir * m_Axis[2]);
		if (rtMath::Fabs(dir * m_Axis[2]) > m_Extents[2] + ld[2]) 
		{
			return false;
		}

		rtVector3 cross = lineDir.cross(dir);

		if (rtMath::Fabs(cross * m_Axis[0]) > m_Extents[1] * ld[2] + m_Extents[2] * ld[1]) 
		{
			return false;
		}

		if (rtMath::Fabs(cross * m_Axis[1]) > m_Extents[0] * ld[2] + m_Extents[2] * ld[0]) 
		{
			return false;
		}

		if (rtMath::Fabs(cross * m_Axis[2]) > m_Extents[0] * ld[1] + m_Extents[1] * ld[0]) 
		{
			return false;
		}

		return true;
	}


	static bool BoxPlaneClip(const float denom, const float numer, float& scale0, float& scale1) 
	{
		if (denom > 0.0f) 
		{
			if (numer > denom * scale1) 
			{
				return false;
			}
			if (numer > denom * scale0) 
			{
				scale0 = numer / denom;
			}
			return true;
		}
		else if (denom < 0.0f) 
		{
			if (numer > denom * scale0) 
			{
				return false;
			}
			if (numer > denom * scale1) 
			{
				scale1 = numer / denom;
			}
			return true;
		}
		else 
		{
			return (numer <= 0.0f);
		}
	}


	bool rtBox::rayIntersection(const rtVector3& start, const rtVector3& dir, float& scale1, float& scale2) const 
	{
		rtVector3 localStart, localDir;

		localStart = (start - m_Center) * m_Axis.transpose();
		localDir = dir * m_Axis.transpose();

		scale1 = -rtMath::INFINITY;
		scale2 = rtMath::INFINITY;
		return	BoxPlaneClip( localDir.x, -localStart.x - m_Extents[0], scale1, scale2) &&
			    BoxPlaneClip(-localDir.x,  localStart.x - m_Extents[0], scale1, scale2) &&
			    BoxPlaneClip( localDir.y, -localStart.y - m_Extents[1], scale1, scale2) &&
			    BoxPlaneClip(-localDir.y,  localStart.y - m_Extents[1], scale1, scale2) &&
			    BoxPlaneClip( localDir.z, -localStart.z - m_Extents[2], scale1, scale2) &&
			    BoxPlaneClip(-localDir.z,  localStart.z - m_Extents[2], scale1, scale2);
	}



	void rtBox::toPoints(rtVector3 points[8]) const
	{
		rtMatrix3 ax;
		rtVector3 temp[4];

		ax[0] = m_Extents[0] * m_Axis[0];
		ax[1] = m_Extents[1] * m_Axis[1];
		ax[2] = m_Extents[2] * m_Axis[2];
		temp[0] = m_Center - ax[0];
		temp[1] = m_Center + ax[0];
		temp[2] = ax[1] - ax[2];
		temp[3] = ax[1] + ax[2];
		points[0] = temp[0] - temp[3];
		points[1] = temp[1] - temp[3];
		points[2] = temp[1] + temp[2];
		points[3] = temp[0] + temp[2];
		points[4] = temp[0] - temp[2];
		points[5] = temp[1] - temp[2];
		points[6] = temp[1] + temp[3];
		points[7] = temp[0] + temp[3];
	}
}
